/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
//commit test
package edu.wpi.first.wpilibj.templates;

// Vision
import edu.wpi.first.wpilibj.camera.AxisCamera;
import edu.wpi.first.wpilibj.camera.AxisCameraException;
import edu.wpi.first.wpilibj.image.NIVisionException;
import edu.wpi.first.wpilibj.image.ColorImage;
import edu.wpi.first.wpilibj.image.MonoImage;
import edu.wpi.first.wpilibj.image.EllipseDescriptor;
import edu.wpi.first.wpilibj.image.EllipseMatch;
import edu.wpi.first.wpilibj.image.CurveOptions;
import edu.wpi.first.wpilibj.image.ShapeDetectionOptions;


/**
 *
 * @author pilgriml
 */

public class RobotVision {
    private AxisCamera camera_ = AxisCamera.getInstance();
    private static EllipseDescriptor        ellipseDescriptor   = new EllipseDescriptor(3, 200, 3, 100);
    private static CurveOptions             curveOptions        = new CurveOptions(0, 40, 1, 25, 15, 15, 10, 1, 0);
    private static ShapeDetectionOptions    shapeOptions        = new ShapeDetectionOptions(ShapeDetectionOptions.IMAQ_GEOMETRIC_MATCH_SHIFT_INVARIANT, 0, 0, 75, 125, 500);
    private static RobotVision instance_ = new RobotVision();
    
    private RobotVision() { }

    public VisionTarget getTarget()
    {
        if (camera_.freshImage())
        {
            try
            {
                ColorImage image = camera_.getImage();
                MonoImage lumPlane = image.getLuminancePlane();
                EllipseMatch[] results = lumPlane.detectEllipses(ellipseDescriptor, curveOptions, shapeOptions, null);

                image.free();
                lumPlane.free();

                System.out.println("Got image, ellipse: " + results.length);

                for (int i=0; i < results.length; i++)
                {
                    System.out.println("Target mx: " + results[i].m_xPos + " my: " + results[i].m_yPos);
                }
            }

            catch (AxisCameraException e)
            {
                e.printStackTrace();
            }

            catch (NIVisionException e)
            {
                e.printStackTrace();
            }
        }

        VisionTarget target = new VisionTarget();

        return target;
    }

    public static RobotVision getInstance()
    {  
        return instance_;
    }




}
